//
// Created by ZhaoXiaoFei on 2022/8/18.
//

#ifndef ESKF_LOCALIZATION_FRONT_END_HPP
#define ESKF_LOCALIZATION_FRONT_END_HPP

#include <ros/ros.h>
#include <glog/logging.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/MagneticField.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include "preDefineData.hpp"
#include "back_end/back_end.hpp"

#include <tf/transform_broadcaster.h>


class LocalizationFrontEnd{
public:
    LocalizationFrontEnd(ros::NodeHandle& nh);

private:
    void imuCallBack(const sensor_msgs::Imu& imu_msg);

    void gpsCallBack(const sensor_msgs::NavSatFix& gps_msg);

    void magCallBack(const sensor_msgs::MagneticField& mag_mag);

private:

    ros::Subscriber imuSub_;
    ros::Subscriber gpsSub_;
    ros::Subscriber magSub_;

    ros::Publisher fusedPosePub_;
    ros::Publisher fusedPathPub_;
    ros::Publisher fusedPixPub_;
    ros::Publisher fusedOdomPub_;

    nav_msgs::Path ros_path_;
    tf::TransformBroadcaster br_;

    std::unique_ptr<LocalizationBackEnd> localizationBackEnd_;

    void addStateToPath(State* state);

    void publishState();

};
#endif //ESKF_LOCALIZATION_FRONT_END_HPP
